﻿using Messages;
using RosSharp;
using System;
using System.Collections.Generic;

namespace RosInterface
{
    public class RosNode : IDisposable
    {
        public readonly string NodeName;
        public readonly NodeHandle NodeH;

        public RosNode(RosConfiguration config, string name)
        {
            NodeName = name;
            ROS.ROS_MASTER_URI = config.MasterUri.ToString();
            ROS.ROS_HOSTNAME = config.HostName;
            ROS.ROS_IP = config.SelfIp;
            ROS.Init(NodeName);
            NodeH = new NodeHandle();
        }

        public Subscriber<T> Subscrib<T>(string topic, RosSharp.CallbackDelegate<T> callback = null) where T : IRosMessage, new()
        {
            return NodeH.Subscribe<T>(topic, 10, callback);
        }

        public Publisher<T> GetPublisher<T>(string topic) where T : IRosMessage, new()
        {
            return NodeH.Advertise<T>(topic, 1);
        }

        public void Dispose()
        {
            ROS.Shutdown();
            ROS.WaitForShutdown();
        }
    }
}
